/*
@file: filters.cpp
@author: ZZH
@date: 2022-05-05
@info: 数字滤波器实现
*/
#include "libFun.h"
#include "fft.h"

/*
@brief 一阶低通滤波器(采用一阶滞后滤波实现)
@param 采样序列, 截止频率
@return
*/
EXPORT void lpf(pFunCallArg_t pArgs, BasicType* output)
{
    int allCalNum = pArgs->allCalNum;
    BasicType* arg0 = static_cast<BasicType*>(pArgs->args[0]);//采样序列
    BasicType factor = *static_cast<BasicType*>(pArgs->args[1]) / pArgs->fs;//滤波系数, 截止频率 / 采样频率
    BasicType last = 0;

    for (int i = 0;i < allCalNum;i++)
    {
        output[i] = factor * arg0[i] + (1 - factor) * last;
        last = output[i];
    }
}

/*
@brief 一阶高通滤波器
@param
@return
*/
EXPORT void hpf(pFunCallArg_t pArgs, BasicType* output)
{
    int allCalNum = pArgs->allCalNum;
    BasicType* arg0 = static_cast<BasicType*>(pArgs->args[0]);//采样序列
    BasicType factor = *static_cast<BasicType*>(pArgs->args[1]) / pArgs->fs;//滤波系数, 截止频率 / 采样频率
    BasicType last = 0;

    for (int i = 1;i < allCalNum;i++)
    {
        output[i] = factor * (last + (arg0[i] - arg0[i - 1]));
        last = output[i];
    }
}

/*
@brief 均值滤波器, 将整个采样序列划分为多个"均值单元", 从而形成新的采样序列, 新的序列里每个单元内部的平均值作为此单元的值加入序列
@param 采样序列, 均值取样点数
@return
*/
EXPORT void average(pFunCallArg_t pArgs, BasicType* output)
{
    int allCalNum = pArgs->allCalNum;
    BasicType* arg0 = static_cast<BasicType*>(pArgs->args[0]);//采样序列
    int sampleNum = *static_cast<BasicType*>(pArgs->args[1]);//每个均值单元包括的点数

    if (sampleNum > allCalNum || sampleNum <= 0)//参数错误
        return;

    int tail = allCalNum % sampleNum;//不满足数量要求的均值单元
    int sampleTimes = allCalNum / sampleNum;//均值单元的数量
    BasicType cellRes = 0;
    int startIndex = 0;
    int endIndex = 0;

    for (int i = 0;i < sampleTimes;i++)
    {
        startIndex = i * sampleNum;
        endIndex = startIndex + sampleNum;

        for (int j = startIndex;j < endIndex;j++)//计算第N个均值单元的总和
            cellRes += arg0[j];

        cellRes /= sampleNum;//计算均值

        for (int j = startIndex;j < endIndex;j++)//以均值作为整个单元的值
            output[j] = cellRes;
    }

    if (0 != tail)//总点数不是采样点数的整数倍, 上面的处理会留下一块不够一个采样单元大小的数据, 这里处理掉
    {
        startIndex = endIndex;
        endIndex = startIndex + tail;

        for (int i = startIndex;i < endIndex;i++)
            cellRes += arg0[i];

        cellRes /= tail;

        for (int i = startIndex;i < endIndex;i++)
            output[i] = cellRes;
    }
}

/*
@brief FIR滤波器, 可使用MATLAB导出的滤波器参数
@param 采样序列, 滤波器参数(可使用read_file将参数读入)
@return
*/
EXPORT void fir(pFunCallArg_t pArgs, BasicType* output)
{
    int allCalNum = pArgs->allCalNum;
    BasicType* arg0 = static_cast<BasicType*>(pArgs->args[0]);//采样序列
    int level = (int) *static_cast<BasicType*>(pArgs->args[1]) + 1;
    BasicType* factor = static_cast<BasicType*>(pArgs->args[2]);

    full_conv(output, arg0, allCalNum, factor, level);
}

/*
@brief FIR滤波器生成器
@param 类型, 低截止频率, 高截止频率, 阶数
@return
*/
EXPORT void gen_fir_coff(pFunCallArg_t pArgs, BasicType* output)
{
    int allCalNum = pArgs->allCalNum;
    unsigned int type = (unsigned int) *static_cast<BasicType*>(pArgs->args[0]);//类型(低通, 高通, 带通, 带阻)
    BasicType lcf = *static_cast<BasicType*>(pArgs->args[1]);//低截止频率(归一化频率)
    BasicType hcf = *static_cast<BasicType*>(pArgs->args[2]);//高截止频率(归一化频率)
    unsigned int order = (unsigned int) *static_cast<BasicType*>(pArgs->args[3]) + 1;//阶数

    if (lcf >= 0.5 || hcf >= 0.5 || type > 3 || order > allCalNum)
        return;

    if (type > 1)//带通或带阻
    {
        if (lcf >= hcf)//无效通带
            return;
    }

    BasicType* hamming_window = new BasicType[order];
    BasicType* filter_coeffs = new BasicType[order];
    hamming(hamming_window, order);

    int fOrder_2 = (int) floor(order / 2);

    switch (type)
    {
        case 0://lp
            for (int i = 0;i < order;i++)
            {
                BasicType x = M_PI * (i - fOrder_2);
                BasicType larg = 2 * lcf * x;
                filter_coeffs[i] = 0 == x ? 2 * lcf : sin(larg) / x;
                output[i] = filter_coeffs[i] * hamming_window[i];
            }
            break;

        case 1://hp
            for (int i = 0;i < order;i++)
            {
                BasicType x = M_PI * (i - fOrder_2);
                BasicType larg = 2 * lcf * x;
                filter_coeffs[i] = 0 == x ? 1 - 2 * lcf : -sin(larg) / x;
                output[i] = filter_coeffs[i] * hamming_window[i];
            }
            break;

        case 2://bp
            for (int i = 0;i < order;i++)
            {
                BasicType x = M_PI * (i - fOrder_2);
                BasicType harg = 2 * hcf * x;
                BasicType larg = 2 * lcf * x;
                filter_coeffs[i] = 0 == x ? 2 * (hcf - lcf) : (sin(harg) - sin(larg)) / x;
                output[i] = filter_coeffs[i] * hamming_window[i];
            }
            break;

        case 3://bs
            for (int i = 0;i < order;i++)
            {
                BasicType x = M_PI * (i - fOrder_2);
                BasicType harg = 2 * hcf * x;
                BasicType larg = 2 * lcf * x;
                filter_coeffs[i] = 0 == x ? 1 - 2 * (hcf - lcf) : (sin(larg) - sin(harg)) / x;
                output[i] = filter_coeffs[i] * hamming_window[i];
            }
            break;
    }

    // memcpy(output, filter_coeffs, order * sizeof(*filter_coeffs));

    delete[] hamming_window;
    delete[] filter_coeffs;
}

LibFunction_t funcs[] = {
    LIB_FUNCTION(lpf, 2),
    LIB_FUNCTION(hpf, 2),
    LIB_FUNCTION(fir, 3),
    LIB_FUNCTION(average, 2),
    LIB_FUNCTION(gen_fir_coff, 4),
    END_OF_LIB
};

register_function_lib(funcs);
